/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: Trip Queue
 *         $Id: tripq.c 27 2014-04-13 22:57:20Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <string.h>
#include "maxq_config.h"
#include "comm/maxq_comm.h"
#include "car.h"
#include "tripq.h"

#define center_hall(width)    wall_dist(width), wall_dist(width)
#define no_walls              -1,   -1

#if 0
// Long Straight Line
const tripq_t tripq[] = {
  //                  Speed         Direction             Distance          Wall
  { FIXED_SPEED, 0, { 700, 700 }, { FORWARD, FORWARD }, { 20000, 20000 }, { no_walls } },
};
#endif

#if 0
// Hallway Navigation
const tripq_t tripq[] = {
  //                  Speed                       Direction             Distance                     Wall
  { FIXED_SPEED,   0, { 200, 600 },             { FORWARD, FORWARD }, {  300,  300 },              { no_walls } },
  { FIXED_SPEED,   0, { 600, 600 },             { FORWARD, FORWARD }, { 4200, 4200 },              { no_walls } },
  { TURNING,       0, { 600, in_whl_spd(600) }, { FORWARD, FORWARD }, { ARC_OUTSIDE, ARC_INSIDE }, { no_walls } },
  { FIXED_SPEED,   0, { 600, 600 },             { FORWARD, FORWARD }, { 3600 - (2 * TURN_LAT_DIST), 3600 - (2 * TURN_LAT_DIST) }, { no_walls } },
  { TURNING,       0, { in_whl_spd(600), 600 }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },
  { FIXED_SPEED,   0, { 600, 600 },             { FORWARD, FORWARD }, { 2000, 2000 },              { no_walls } },
  { FIXED_SPEED,   0, { 400, 400 },             { FORWARD, FORWARD }, {  300,  300 },              { no_walls } },
  { FIXED_SPEED,   0, { 200, 200 },             { FORWARD, FORWARD }, {  200,  200 },              { no_walls } }
};
#endif

#if 0
// Test Track
#define TARGET_SPEED  600
#define HALL_WIDTH    122
const tripq_t tripq[] = {
  //                  Speed                                      Direction             Distance                     Wall
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 4853, 4853 },              { center_hall(HALL_WIDTH) } },
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 1289, 1289 },              { center_hall(HALL_WIDTH) } },
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 1257, 1257 },              { center_hall(HALL_WIDTH) } },
  { TURNING,     0, { TARGET_SPEED, in_whl_spd(TARGET_SPEED)}, { FORWARD, FORWARD }, { ARC_OUTSIDE, ARC_INSIDE }, { no_walls } },
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 1220, 1220 },              { center_hall(HALL_WIDTH) } },
};
#endif

#if 0
// Race Course
#define TARGET_SPEED  600
#define HALL_WIDTH    122

#if 0
// IDEAL TRACK
const tripq_t tripq[] = {
  //                  Speed                                      Direction             Distance                     Wall
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 5791, 5791 },              { center_hall(HALL_WIDTH) } },  // 19' straight
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },                 // turn left
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 4877, 4877 },              { center_hall(HALL_WIDTH) } },  // 16' straight
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },                 // turn left
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 1219, 1219 },              { center_hall(HALL_WIDTH) } },  // 4' straight
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },                 // turn left
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 2438, 2438 },              { center_hall(HALL_WIDTH) } },  // 8' straight
  { TURNING,     0, { TARGET_SPEED, in_whl_spd(TARGET_SPEED)}, { FORWARD, FORWARD }, { ARC_OUTSIDE, ARC_INSIDE }, { no_walls } },                 // turn right
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, {  609,  609 },              { center_hall(HALL_WIDTH) } },  // 2' straight
  { TURNING,     0, { TARGET_SPEED, in_whl_spd(TARGET_SPEED)}, { FORWARD, FORWARD }, { ARC_OUTSIDE, ARC_INSIDE }, { no_walls } },                 // turn right
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 2438, 2438 },              { center_hall(HALL_WIDTH) } },  // 8' straight
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },                 // turn left
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, {  457,  457 },              { center_hall(HALL_WIDTH) } },  // 1.5' straight
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },                 // turn left
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 2438, 2438 },              { center_hall(HALL_WIDTH) } },  // 8' straight (wide hallway)
  { TURNING,     0, { TARGET_SPEED, in_whl_spd(TARGET_SPEED)}, { FORWARD, FORWARD }, { ARC_OUTSIDE, ARC_INSIDE }, { no_walls } },                 // turn right
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 1219, 1219 },              { center_hall(HALL_WIDTH) } },  // 4' straight
};
#else
// MEASURED TRACK
const tripq_t tripq[] = {
  //                  Speed                                      Direction             Distance                     Wall
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 5943, 5943 },              { center_hall(119) } },         // 234" straight
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },                 // turn left
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 4953, 4953 },              { center_hall(117) } },         // 195" straight
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },                 // turn left
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 1289, 1289 },              { center_hall(117) } },         // 50.75" straight
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },                 // turn left
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 2496, 2496 },              { center_hall(122) } },         // 98.25" straight
  { TURNING,     0, { TARGET_SPEED, in_whl_spd(TARGET_SPEED)}, { FORWARD, FORWARD }, { ARC_OUTSIDE, ARC_INSIDE }, { no_walls } },                 // turn right
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, {  679,  679 },              { center_hall(122) } },         // 26.75" straight
  { TURNING,     0, { TARGET_SPEED, in_whl_spd(TARGET_SPEED)}, { FORWARD, FORWARD }, { ARC_OUTSIDE, ARC_INSIDE }, { no_walls } },                 // turn right
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 2477, 2477 },              { center_hall(122) } },         // 97.5" straight
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },                 // turn left
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, {  533,  533 },              { center_hall(117) } },         // 21" straight
  { TURNING,     0, { in_whl_spd(TARGET_SPEED),TARGET_SPEED }, { FORWARD, FORWARD }, { ARC_INSIDE, ARC_OUTSIDE }, { no_walls } },                 // turn left
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 2438, 2438 },              { center_hall(152) } },         // 8' straight (wide hallway)
  { TURNING,     0, { TARGET_SPEED, in_whl_spd(TARGET_SPEED)}, { FORWARD, FORWARD }, { ARC_OUTSIDE, ARC_INSIDE }, { no_walls } },                 // turn right
  { FIXED_SPEED, 0, { TARGET_SPEED, TARGET_SPEED },            { FORWARD, FORWARD }, { 1219, 1219 },              { center_hall(HALL_WIDTH) } },  // 4' straight
};
#endif
#endif

#if 0
const tripq_t tripq[] = {
  //                  Speed         Direction             Distance          Wall
  { FIXED_SPEED, 0, { 600, 600 }, { FORWARD, FORWARD }, { 2000, 2000 }, { no_walls } },
  { PAUSE,       0, {   0,   0 }, { FORWARD, FORWARD }, {    0,    0 }, { no_walls } },
  { FIXED_VALUE, 0, { VALUE_ROTATE, VALUE_ROTATE }, { FORWARD, REVERSE }, { DIST_ROTATE_180_DEG, -DIST_ROTATE_180_DEG }, { no_walls } },
  { PAUSE,       0, {   0,   0 }, { FORWARD, FORWARD }, {    0,    0 }, { no_walls } },

  { FIXED_SPEED, 0, { 600, 600 }, { FORWARD, FORWARD }, { 2000, 2000 }, { no_walls } },
  { PAUSE,       0, {   0,   0 }, { FORWARD, FORWARD }, {    0,    0 }, { no_walls } },
  { FIXED_VALUE, 0, { VALUE_ROTATE, VALUE_ROTATE }, { REVERSE, FORWARD }, { -DIST_ROTATE_180_DEG, DIST_ROTATE_180_DEG }, { no_walls } },
  { PAUSE,       0, {   0,   0 }, { FORWARD, FORWARD }, {    0,    0 }, { no_walls } },

  { FIXED_SPEED, 0, { 600, 600 }, { FORWARD, FORWARD }, { 2000, 2000 }, { no_walls } },
  { PAUSE,       0, {   0,   0 }, { FORWARD, FORWARD }, {    0,    0 }, { no_walls } },
  { FIXED_VALUE, 0, { VALUE_ROTATE, VALUE_ROTATE }, { FORWARD, REVERSE }, { DIST_ROTATE_180_DEG, -DIST_ROTATE_180_DEG }, { no_walls } },
  { PAUSE,       0, {   0,   0 }, { FORWARD, FORWARD }, {    0,    0 }, { no_walls } },

  { FIXED_SPEED, 0, { 600, 600 }, { FORWARD, FORWARD }, { 2000, 2000 }, { no_walls } },
  { PAUSE,       0, {   0,   0 }, { FORWARD, FORWARD }, {    0,    0 }, { no_walls } },
  { FIXED_VALUE, 0, { VALUE_ROTATE, VALUE_ROTATE }, { REVERSE, FORWARD }, { -DIST_ROTATE_180_DEG, DIST_ROTATE_180_DEG }, { no_walls } },
  { PAUSE,       0, {   0,   0 }, { FORWARD, FORWARD }, {    0,    0 }, { no_walls } },
};
#endif

const unsigned int tq_size = sizeof(tripq) / sizeof(tripq_t);
int tq_index;
